import cv2
import numpy as np
import time
import math
import global_var as glv
import visual_function as vf
from basic_game import basic_game


class pit(basic_game):
    def __init__(self, running=False):
        ###云台初始化###
        self.PWM_Y = 1700  # 云台角度初始化
        basic_game.__init__(self)
        self.state = "change_PWM"
        basic_game.execute(self, self.PWM_Y)
        self.state = "change_PWM_X"
        basic_game.execute(self, 1500)
        ###标志位###
        self.go_yellow = True  # 走去黄色的坑道
        self.go_hole = False  # 走上黄色坑道之后，走到洞附近
        self.go_edge = False  # 走到洞附近后，走到边界
        self.go_time = 0  # 直走次数
        self.go_state = 0  # 走到哪个状态
        self.throughGate = False  # 已通过该关卡
        self.headUp = True  # 抬头调整位姿-大
        self.headMed = False  # 抬头调整位姿-小
        ###阈值###
        self.target_color = "orange"
        self.perY = 100
        self.PWM_MaxY = 2300
        self.PWM_MinY = 1700
        self.PWM_MedY = 2000
        self.minCenter = self.resize_width / 2 - self.resize_width / 8  # 中心区域左边界
        self.maxCenter = self.resize_width / 2 + self.resize_width / 8  # 中心区域右边界
        self.threahold_step_down = self.resize_height - self.resize_height / 3  # 执行下台阶动作阈值
        self.threahold_step_up = self.resize_height - self.resize_height / 3  # 上台阶指令阈值
        self.threahold_change_PWM = self.resize_height - self.resize_height / 3  # 在云台未到目标值时目标接近需要更高云台的阈值
        ###变量####
        self.lineList = None
        self.center_x = None
        self.Lines = None

    def go_yellow(self):
        # 得到目标物的信息
        if self.get_color:
            self.get_color = False
            # 没有上黄色的坑道后
            # 两步一抬头
            # 调整位置（抬头看黄色的两个边界）
            # 低头直走 判断有没有到黄色的坑道
            # 立正

            # 抬头：判断坑道的中心在不在图像的中心,不在的话调整
            if self.headUp:
                if self.center_x > self.minCenter and self.center_x < self.maxCenter:
                    print("位姿正确，走两步，低头")
                    self.state = "go_straight"
                    basic_game.execute(self, executeNum=2)
                    self.state = "change_PWM"
                    basic_game.execute(self, self.PWM_MaxY)
                    self.headUp = False
                else:
                    if self.center_x > self.resize_width / 2:
                        print("位姿错误，向左调整")
                        self.state = "turn_left"
                        basic_game.execute(self)
                    else:
                        print("位姿错误，向右调整")
                        self.state = "turn_right"
                        basic_game.execute(self)
            # 低头：判断有没有到黄色的坑道边界
            else:
                if self.lineList["down_y"] > self.threahold_change_PWM:
                    print("已经到达黄色的坑道边界！立正，抬头")
                    self.PWM_Y = self.PWM_MinY
                    self.state = "change_PWM"
                    basic_game.execute(self, self.PWM_Y)
                    self.headUp = True
                    self.go_yellow = False
                    self.go_hole = True
                else:
                    print("还差一点就到黄色的坑道边界！抬头走两步试试吧。")
                    self.state = "change_PWM"
                    basic_game.execute(self, self.PWM_Y)
                    self.headUp = True
        # 没有得到目标物的信息
        else:
            # 把头低下100，直到看到目标物
            if self.headUp:
                print("抬头都没有发现目标物体，降一下角度试试")
                self.PWM_Y += 100
                self.state = "change_PWM"
                basic_game.execute(self, self.PWM_Y)
            else:
                print("低头没有发现目标物体，还没有到达边界，直接抬头")
                self.state = "change_PWM"
                basic_game.execute(self, self.PWM_Y)
                self.headUp = True

    def go_hole(self):
        # 得到目标物的信息
        if self.get_color:
            self.get_color = False
            # 上了黄色坑道
            # 调整位置（抬头开始判断黑色下边线的上下距离）
            # 低头直走 判断有没有到黑色边界
            # 右转，立正

            if self.headUp:
                if self.center_x > self.minCenter and self.center_x < self.maxCenter:
                    print("位姿正确，走两步，低头")
                    self.state = "go_straight"
                    basic_game.execute(self, executeNum=2)
                    self.state = "change_PWM"
                    basic_game.execute(self, self.PWM_MaxY)
                    self.headUp = False
                else:
                    if self.center_x > self.resize_width / 2:
                        print("位姿错误，向左调整")
                        self.state = "turn_left"
                        basic_game.execute(self)
                    else:
                        print("位姿错误，向右调整")
                        self.state = "turn_right"
                        basic_game.execute(self)
            # 低头：判断有没有到黄色的坑道边界
            else:
                if self.lineList["down_y"] > self.threahold_change_PWM:
                    print("已经到达黑色坑的边界！立正，抬头，右转一波！")
                else:
                    print("还差一点就到黑色坑的边界！直接走一步。")
                    self.state = "go_straight"
                    basic_game.execute(self)
                    self.headUp = True

                self.state = "attention"
                basic_game.execute(self)

                self.PWM_Y = self.PWM_MinY
                self.state = "change_PWM"
                basic_game.execute(self, self.PWM_Y)

                self.state = "big_turn_right"
                basic_game.execute(self)

                self.headUp = True
                self.go_hole = False
                self.go_edge = True
        # 没有得到目标物的信息
        else:
            # 把头低下100，直到看到目标物
            if self.headUp:
                print("抬头都没有发现目标物体，降一下角度试试")
                self.PWM_Y += 100
                self.state = "change_PWM"
                basic_game.execute(self, self.PWM_Y)
            else:
                print("低头没有发现目标物体，还没有到达边界，直接抬头")
                self.state = "change_PWM"
                basic_game.execute(self, self.PWM_Y)
                self.headUp = True

    def go_edge(self):
        # 调整位置（低头看黄色上边界是否水平，如果不水平，立正微调）
        # 低头直走 判断有没有到黄色上边界

        # 看得到黄色的边界，调整位姿  抬头的话就调整位姿，低头的话也调整位姿之后判断距离
        if self.get_color:
            self.get_color = False

            if -0.1 < self.lineList["up_k"] < 0.1:  # 水平
                print("go times=1")
            elif self.lineList["up_k"] >= 0.1:  # 机器人方向偏左
                print("右转")
            elif self.lineList["up_k"] <= -0.1:  # 机器人方向偏右
                print("左转")

            if self.headUp:
                # 走多远，如果是最低的情况,判断距离
                if self.PWM_Y >= self.PWM_MaxY:
                    print("低头判断")
                    self.headUp = False

                if self.PWM_Y <= self.PWM_MaxY:
                    print("走一步,低头判断")
                    self.headUp = False
            else:
                if self.lineList["up_y"] > self.threahold_step_down:
                    print("立正")
                else:
                    self.headUp = True

        # 看不到边缘的情况,抬头就降100，低头就
        else:
            if self.headUp:
                if self.PWM_Y <= self.PWM_MaxY:
                    self.PWM_Y += 100
                    self.state = "change_PWM"
                    basic_game.execute(self, self.PWM_Y)


    def run(self, hsvImage, outImage=None, state=0):
        #############从看到任务到，侧滑之前的调整##################################
        if self.go_yellow:
            self.get_color, self.lineList, self.center_x = vf.getAreaByColor(hsvImage, self.target_color, outImage)
            print(self.get_color, self.center_x)
            if outImage is not None:
                cv2.imshow("stepframe", outImage)
                cv2.waitKey(1)
            pit.go_yellow(self)
            return False
        if self.go_yellow is False and self.go_hole:
            self.get_color, self.lineList, self.center_x = vf.getAreaByColor(hsvImage, 'black', outImage)
            if outImage is not None:
                cv2.imshow("stepframe", outImage)
                cv2.waitKey(1)
            pit.go_hole(self)
            return False
        if self.go_hole is False and self.go_edge:
            self.get_color, self.lineList, self.center_x = vf.getAreaByColor(hsvImage, self.target_color, outImage)
            if outImage is not None:
                cv2.imshow("stepframe", outImage)
                cv2.waitKey(1)
            pit.go_edge(self)
            return False
        #############################################################################
        ######完成之后,侧滑一套动作组###########################
        if self.go_edge is False:
            print("ssr.run_group")
            self.throughGate = True
        if self.throughGate:
            return True

# 没有上黄色的坑道后（标志位）
# 两步一抬头
# 调整位置（抬头看黄色的两个边界）
# 低头直走 判断有没有到黄色的坑道
# 立正

# 上了黄色坑道
# 调整位置（抬头开始判断黑色下边线的上下距离）
# 低头直走 判断有没有到黑色边界
# 右转，立正

# 调整位置（低头看黄色上边界是否水平，如果不水平，立正微调）
# 低头直走 判断有没有到黄色上边界
# 立正 侧滑  后退 左转


#
